#pragma once
#include "hybrid_a_star.h"
#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/path.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <vector>

using namespace std;

class Replanning {
private:
    bool                              bHasMap = false;
    vector< vector< bool > >          map_data;
    vector< vector< bool > >          static_map_data;
    nav_msgs::msg::OccupancyGrid::Ptr image_map;

    int                       motion_type = 0;
    double                    ref_vel     = 0;
    vector< traj >            last_ref_traj;  // 重规划前的参考轨迹
    vector< Eigen::Vector3d > normal_obstacles;
    vector< Eigen::Vector3d > vehicle_obstacles;
    Eigen::Vector3d           start_state, target_state;  // 重规划起点与终点
    nav_msgs::msg::Path       replanning_path;

public:
    Replanning();

    ~Replanning();

    inline bool isExistMap() {
        return bHasMap;
    }

    /**
     * @brief 计算代价地图每个栅格是否被障碍物占据，占据数值为100，未被占据数值为0，未知区域为-1
     */
    void calcGridmapData( const nav_msgs::msg::OccupancyGrid::Ptr image_map );

    /**
     * @brief 接收到障碍物信息后更新代价地图中的障碍物占据数据值
     */
    void updateGridmap( const vector< Eigen::Vector3d > &normal_obstacles,
                        const vector< Eigen::Vector3d > &vehicle_obstacles );

    /**
     * @brief 计算车辆占据的栅格
     */
    void getVehGridMap( const Eigen::Vector3d &veh_obs_pos );

    /**
     * @brief 获取重规划前的参考轨迹
     */
    void setRefTraj( const vector< traj > &last_ref_traj_ );

    bool isPointInRectangle( const Eigen::Vector2d &p, const Eigen::Vector3d &veh_obs );

    /**
     * @brief 判断两车辆是否碰撞
     * @return true为碰撞
     */
    bool checkCollision( const Eigen::Vector3d &veh_obs, const traj &curr_pose );

    /**
     * @brief 从车辆当前位置开始按照原始参考轨迹模拟运动，如果某时刻车辆落在障碍物占据的栅格内，则需要重规划
     * @return 返回true为激活重规划
     */
    bool isActiveReplanning( const traj &current_pos_ );

    /**
     * @brief 寻找重规划的起点和终点，起点一般为车辆当前位置之后几个索引的的状态，终点不是很好确定（问题所在）
     */
    std::vector< int > findStartAndTargetState( const traj &current_pose );

    void findStartAndTargetState( const Eigen::Vector3d &start_, const Eigen::Vector3d &goal_, int motion_type_ );

    int calculateNewStartState( int count, int histoty_start_index, double reverse_dist );

    /**
     * @brief 调用重规划算法（接口）
     */
    bool runReplanning();

    void getReplanningPath( nav_msgs::msg::Path &waypoints_path );
};